#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

/*
发布方实现，需要订阅乌龟的位置信息，转换为相对于窗体的坐标关系，并发布
准备：
    话题： /turtle1/pose
    消息：/turtlesim/Pose
实现：
    1， 头文件
    2，初始化操作
    3，创建订阅对象 /turtle1/pose
    4，回调函数处理订阅的消息，并发布；
    5，回旋函数
*/

void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    //获取位姿消息，转换为坐标系相对关系，并发布
    //a， 创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    //b， 组织被发布数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id="world";
    ts.header.stamp=ros::Time::now();
    ts.child_frame_id="turtle1";
    //坐标系需要设置
    ts.transform.translation.x=pose->x;
    ts.transform.translation.y=pose->y;
    ts.transform.translation.z=0;

    //坐标的四元数（通过欧拉角转换
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x=qtn.getX();
    ts.transform.rotation.y=qtn.getY();
    ts.transform.rotation.z=qtn.getZ();
    ts.transform.rotation.w=qtn.getW();
    //c， 发布
    pub.sendTransform(ts);
}


int main(int argc, char** argv)
{
    //2，初始化操作
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;
    //3，创建订阅对象 /turtle1/pose
    ros::Subscriber sub=nh.subscribe("/turtle1/pose", 100, doPose);
    //4，回调函数处理订阅的消息，并发布；
    //5，回旋函数
    ros::spin();

    return 0;
}